#include "pch.h"
#include "DistanceConstraint.h"
#include "PositionBasedDynamics.h"

bool CDistanceConstraint::initializeConstraint(CParticleData& vParticleData, const std::pair<int, int>& vParticlePair, double vStiffness)
{
	m_stiffness = vStiffness;

	m_ParticleIndices[0] = vParticlePair.first;
	m_ParticleIndices[1] = vParticlePair.second;

	Eigen::Vector3d x1_0 = vParticleData.getPosition0(vParticlePair.first);
	Eigen::Vector3d x2_0 = vParticleData.getPosition0(vParticlePair.second);

	m_restDistance = (x2_0 - x1_0).norm();

	return true;
}

bool CDistanceConstraint::solveConstraintV(CParticleData& vParticleData)
{
	const unsigned i1 = m_ParticleIndices[0];
	const unsigned i2 = m_ParticleIndices[1];

	Eigen::Vector3d &x1 = vParticleData.getPosition(i1);
	Eigen::Vector3d &x2 = vParticleData.getPosition(i2);
	const double invMass1 = vParticleData.getInvMass(i1);
	const double invMass2 = vParticleData.getInvMass(i2);

	Eigen::Vector3d corr1, corr2;
	const bool res = CPositionBasedDynamics::solve_DistanceConstraint(
		x1, invMass1, x2, invMass2,
		m_restDistance, m_stiffness, m_stiffness, corr1, corr2);

	_ASSERTE(res == true);
	if (res)
	{
		if (invMass1 != 0.0)
			x1 += corr1;
		if (invMass2 != 0.0)
			x2 += corr2;
	}
	return res;
}